import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.TachoPilot;
import Interface.BluetoothNXT;

 

public class TestBT {

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 164;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		TachoPilot pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER)
		{		
			BluetoothNXT test = new BluetoothNXT(pilot);
			test.start();
		}
		
		
	//test.sendValue((byte)5, (byte)1);
	for (int i = 0; i < 100; i++) {
		pilot.forward();
	}
	pilot.rotate(90);
	for (int i = 0; i < 100; i++) {
		pilot.forward();
	}
		 

	}

}
